#include "main.h"
#include "lift.h"

void motor_lift_up_setup (TIM_HandleTypeDef *htim,int FREQ)
{
    //DIR
    //Set the direction first
    HAL_GPIO_WritePin(LIFT_0_DIR_GPIO_Port,LIFT_0_DIR_Pin,GPIO_PIN_SET);
    HAL_GPIO_WritePin(LIFT_1_DIR_GPIO_Port,LIFT_1_DIR_Pin,GPIO_PIN_SET);
    HAL_GPIO_WritePin(LIFT_2_DIR_GPIO_Port,LIFT_2_DIR_Pin,GPIO_PIN_SET);
    HAL_GPIO_WritePin(LIFT_3_DIR_GPIO_Port,LIFT_3_DIR_Pin,GPIO_PIN_SET);
    //ENABLE
    HAL_GPIO_WritePin (LIFT_0_EN_GPIO_Port, LIFT_0_EN_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin (LIFT_1_EN_GPIO_Port, LIFT_1_EN_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin (LIFT_2_EN_GPIO_Port, LIFT_2_EN_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin (LIFT_3_EN_GPIO_Port, LIFT_3_EN_Pin, GPIO_PIN_RESET);


    HAL_TIM_Base_Start(htim);
    HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(htim, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(htim, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(htim, TIM_CHANNEL_4);

    __HAL_TIM_SetCompare(htim, TIM_CHANNEL_1, ((FREQ+1)/2-1));
    __HAL_TIM_SetCompare(htim, TIM_CHANNEL_2, ((FREQ+1)/2-1));
    __HAL_TIM_SetCompare(htim, TIM_CHANNEL_3, ((FREQ+1)/2-1));
    __HAL_TIM_SetCompare(htim, TIM_CHANNEL_4, ((FREQ+1)/2-1));
}

void motor_lift_up_check (TIM_HandleTypeDef *htim)
{
    GPIO_PinState new_l0 = HAL_GPIO_ReadPin (LIFT_0_UP_GPIO_Port, LIFT_0_UP_Pin);
    GPIO_PinState new_l1 = HAL_GPIO_ReadPin (LIFT_1_UP_GPIO_Port, LIFT_1_UP_Pin);
    GPIO_PinState new_l2 = HAL_GPIO_ReadPin (LIFT_2_UP_GPIO_Port, LIFT_2_UP_Pin);
    GPIO_PinState new_l3 = HAL_GPIO_ReadPin (LIFT_3_UP_GPIO_Port, LIFT_3_UP_Pin);

//  printf ("LIFT_UP: %d, %d, %d, %d\r\n", new_l0, new_l1, new_l2, new_l3);

    if(!new_l0)
    {
        HAL_TIM_PWM_Stop(htim,TIM_CHANNEL_1);
        //printf ("LIFT[0] stopped.\r\n");
    }

    if(!new_l1)
    {
        HAL_TIM_PWM_Stop(htim,TIM_CHANNEL_2);
        //printf ("LIFT[1] stopped.\r\n");
    }

    if(!new_l2)
    {
        HAL_TIM_PWM_Stop(htim,TIM_CHANNEL_3);
        //printf ("LIFT[2] stopped.\r\n");
    }

    if(!new_l3)
    {
        HAL_TIM_PWM_Stop(htim,TIM_CHANNEL_4);
        //printf ("LIFT[3] stopped.\r\n");
    }
}
void motor_lift_down_setup (TIM_HandleTypeDef *htim,int FREQ)
{
    //DIR
    //HAL_GPIO_WritePin (GPIOB, GPIO_PIN_6|GPIO_PIN_7|GPIO_PIN_8|GPIO_PIN_9, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(LIFT_0_DIR_GPIO_Port,LIFT_0_DIR_Pin,GPIO_PIN_RESET);
    HAL_GPIO_WritePin(LIFT_1_DIR_GPIO_Port,LIFT_1_DIR_Pin,GPIO_PIN_RESET);
    HAL_GPIO_WritePin(LIFT_2_DIR_GPIO_Port,LIFT_2_DIR_Pin,GPIO_PIN_RESET);
    HAL_GPIO_WritePin(LIFT_3_DIR_GPIO_Port,LIFT_3_DIR_Pin,GPIO_PIN_RESET);
    //ENABLE
    //HAL_GPIO_WritePin (GPIOB, GPIO_PIN_2|GPIO_PIN_3|GPIO_PIN_4|GPIO_PIN_5, GPIO_PIN_RESET);
    HAL_GPIO_WritePin (LIFT_0_EN_GPIO_Port, LIFT_0_EN_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin (LIFT_1_EN_GPIO_Port, LIFT_1_EN_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin (LIFT_2_EN_GPIO_Port, LIFT_2_EN_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin (LIFT_3_EN_GPIO_Port, LIFT_3_EN_Pin, GPIO_PIN_RESET);


    HAL_TIM_Base_Start(htim);
    HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(htim, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(htim, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(htim, TIM_CHANNEL_4);

    __HAL_TIM_SetCompare(htim, TIM_CHANNEL_1, ((FREQ+1)/2-1));
    __HAL_TIM_SetCompare(htim, TIM_CHANNEL_2, ((FREQ+1)/2-1));
    __HAL_TIM_SetCompare(htim, TIM_CHANNEL_3, ((FREQ+1)/2-1));
    __HAL_TIM_SetCompare(htim, TIM_CHANNEL_4, ((FREQ+1)/2-1));
}

void motor_lift_down_check (TIM_HandleTypeDef *htim)
{
    GPIO_PinState new_l0 = HAL_GPIO_ReadPin (LIFT_0_DOWN_GPIO_Port, LIFT_0_DOWN_Pin);
    GPIO_PinState new_l1 = HAL_GPIO_ReadPin (LIFT_1_DOWN_GPIO_Port, LIFT_1_DOWN_Pin);
    GPIO_PinState new_l2 = HAL_GPIO_ReadPin (LIFT_2_DOWN_GPIO_Port, LIFT_2_DOWN_Pin);
    GPIO_PinState new_l3 = HAL_GPIO_ReadPin (LIFT_3_DOWN_GPIO_Port, LIFT_3_DOWN_Pin);

//  printf ("LIFT_DN: %d, %d, %d, %d\r\n", new_l0, new_l1, new_l2, new_l3);

    if(!new_l0)
    {
        HAL_TIM_PWM_Stop(htim,TIM_CHANNEL_1);
        HAL_GPIO_WritePin(LIFT_0_EN_GPIO_Port,LIFT_0_EN_Pin,GPIO_PIN_SET);

        //printf ("LIFT[0] stopped.\r\n");
    }

    if(!new_l1)
    {
        HAL_TIM_PWM_Stop(htim,TIM_CHANNEL_2);
        HAL_GPIO_WritePin(LIFT_1_EN_GPIO_Port,LIFT_1_EN_Pin,GPIO_PIN_SET);

        //printf ("LIFT[1] stopped.\r\n");
    }

    if(!new_l2)
    {
        HAL_TIM_PWM_Stop(htim,TIM_CHANNEL_3);
        HAL_GPIO_WritePin(LIFT_2_EN_GPIO_Port,LIFT_2_EN_Pin,GPIO_PIN_SET);

        //printf ("LIFT[2] stopped.\r\n");
    }

    if(!new_l3)
    {
        HAL_TIM_PWM_Stop(htim,TIM_CHANNEL_4);
        HAL_GPIO_WritePin(LIFT_3_EN_GPIO_Port,LIFT_3_EN_Pin,GPIO_PIN_SET);

        //printf ("LIFT[3] stopped.\r\n");
    }
}

void motor_lift_stop(TIM_HandleTypeDef *htim)
{
    HAL_TIM_PWM_Stop(htim,TIM_CHANNEL_1);
    HAL_TIM_PWM_Stop(htim,TIM_CHANNEL_2);
    HAL_TIM_PWM_Stop(htim,TIM_CHANNEL_3);
    HAL_TIM_PWM_Stop(htim,TIM_CHANNEL_4);


}

void motor_lift_enable(void)
{

    HAL_GPIO_WritePin(LIFT_0_EN_GPIO_Port,LIFT_0_EN_Pin,GPIO_PIN_RESET);
    HAL_GPIO_WritePin(LIFT_1_EN_GPIO_Port,LIFT_1_EN_Pin,GPIO_PIN_RESET);
    HAL_GPIO_WritePin(LIFT_2_EN_GPIO_Port,LIFT_2_EN_Pin,GPIO_PIN_RESET);
    HAL_GPIO_WritePin(LIFT_3_EN_GPIO_Port,LIFT_3_EN_Pin,GPIO_PIN_RESET);

}


